#include "../handler_task.h"
#include "../../driver/arm/arm_motor.h"
#include "../../driver/arm/elevator_motor.h"
#include "../../driver/arm/parts.h"
#include "../../driver/arm/sensor.h"
#include "../../service/salve_conn.h"
#include "../../conn/proto.h"
#include "../../service/arm/pick_service.h"

namespace task {

using namespace conn;
using namespace driver;
using namespace utils;
using namespace service;

static Error on_elevator_ctrl(const Message &msg) {
    uint16_t mm;
    Error err = msg.parse(mm);
    if (err) {
        goto finish;
    }
    err = elevatorMotor->run_with_mm(mm);
finish:
    return err;
}

static Error on_jaw_ctrl(const Message &msg) {
    uint8_t type;
    Error err = msg.parse(type);
    if (err) {
        goto finish;
    }
    switch (type) {
        case 0: parts->jaw_open(); break;
        case 1: parts->jaw_close(); break;
        case 2: parts->jaw_release(); break;
        default: err = ec::INVALID; break;
    }
finish:
    return err;
}

static Error on_arm_ctrl(const Message &msg) {
    uint8_t type;
    Error err = msg.parse(type);
    if (err) {
        goto finish;
    }
    switch (type) {
        case 0: err = armMotor->enable(); break;
        case 1: err = armMotor->disable(); break;
        case 2: armMotor->status_update(); break;
        case 3: err = armMotor->adj(); break;
        default: err = ec::INVALID; break;
    }
finish:
    return err;
}

static Error on_arm_run_ctrl(const Message &msg) {
    uint16_t p1, p2, speed;
    Error err = msg.parse(p1, p2, speed);
    if (err) {
        goto finish;
    }
    err = armMotor->run(p1, p2, speed);
finish:
    return err;
}

static Error on_film_ctrl() {
    Error err = parts->do_film();
    return err;
}

void HandlerTask::dispatch_impl(const Message &msg) {
    Error err;

    switch (msg.conn_info.req) {

        case proto::REQ_ARM_ELEVATOR_CTRL:
            err = on_elevator_ctrl(msg);
        break;

        case proto::REQ_ARM_JAW_CTRL:
            err = on_jaw_ctrl(msg);
        break;

        case proto::REQ_ARM_ARM_CTRL:
            err = on_arm_ctrl(msg);
        break;

        case proto::REQ_ARM_ARM_RUN_CTRL:
            err = on_arm_run_ctrl(msg);
        break;

        case proto::REQ_ARM_FILM_CTRL:
            err = on_film_ctrl();
        break;

        case proto::REQ_ARM_INIT:
            err = init();
        break;

        

        default:
            err = ec::INVALID;
        break;
    }
    salveConn->response(msg, err);
}

}
